
import time

from DShotPIO import DShotPIO, DSHOT_SPEEDS
from machine import ADC, UART, Pin
from funcs import remap, readCmd, initDMP

# Util function to remap one range into another
def remap(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min

# Create the DShot object 
dshot = DShotPIO(stateMachineID=0, outputPin=17, dshotSpeed=DSHOT_SPEEDS.DSHOT300)

# Most ESCs need a period of zero throttle to arm
for _ in range(100):
    dshot.sendThrottleCommand(48)
    time.sleep_ms(10)

potentiometerValue = 65536

throttle = 0

uart = UART(1, baudrate = 115200,bits = 8,parity = None,stop = 1,
     timeout=10, timeout_char=1, tx = Pin(8),rx = Pin(9))

v0 = 0

while True:
    # Get the potentiometer value and remap it to 48 - 2047
    target = int(remap(potentiometerValue, 0, 65536, 48, 2047))

    vals = readCmd(uart)

    if vals:
        
        (v0,v1,v2,v3, cmd) = vals

    
    # Get the potentiometer value and remap it to 48 - 2047
    target = int(remap(6000, 0, 65536, 48, 2047))
        
    # Drastically changing the throttle will cause problems on a physical motor
    # This is a simple way to slowly increase or decrease throttle towards a target
    if(target > throttle):
        throttle += 1
    elif(target < throttle):
        throttle -= 1
    
    dshot.sendThrottleCommand(throttle)

    time.sleep_ms(2)

